#include "WProgram.h"
// Ted Carancho's MikroQuadAero - September 2008
// An Arudino based quadrocopter using the Sparkfun
// 5DOF IMU and the Pololu RC servo controller
// This version will look at only gyro measurements
// to provide flight stability

// ******************** Initialize Variables ********************
//Debug data
#define DEBUG 1
#define BAUD 38400

// Define pin assignments
#define LEDPIN 13
#define RESETPIN 12
#define THROTTLEPIN 2
#define ROLLPIN 3
#define PITCHPIN 4
#define YAWPIN 5
#define GEARPIN 6
#define AUXPIN 7
#define ROLLRATEPIN 4
#define PITCHRATEPIN 3
#define YAWRATEPIN 5

// Define servo assignments
#define FRONTCHANNEL 0
#define RIGHTCHANNEL 1
#define REARCHANNEL 2
#define LEFTCHANNEL 3

// Define transmitter commands
#define TIMEOUT 4500
void setup();
void loop ();
int findZero(int channel);
void interruptHandler();
void motorCommand(byte servo, unsigned int angle);
void motorConfigure(byte servo);
float updatePID(float targetPosition, float currentPosition, float *lastPosition, float *iState);
int digitalSmooth(int rawIn, int *sensSmoothArray);
volatile int throttle = 0;
volatile int roll = 0;
volatile int pitch = 0;
volatile int yaw = 0;
volatile int gear = 0;
volatile int aux= 0;
int rollCommand, pitchCommand, yawCommand;

// Define motor commands
unsigned int frontCommand = 0;
unsigned int rightCommand = 0;
unsigned int rearCommand = 0;
unsigned int leftCommand = 0;
unsigned int throttleMotorCommand = 0;
int rollMotorCommand = 0;
int pitchMotorCommand = 0;
int yawMotorCommand = 0;

// Gyro setup
// If AREF = 3.3V, then A/D is 931 at 3V and 465 = 1.5V
// Gyro range is from +/-500 deg/sec
float gyroMid = 465;
int rollGyroZero, pitchGyroZero, yawGyroZero;
int rollRate, pitchRate, yawRate;

// Scale transmitter commands (1000 to 2000) to gyro rate (0 to 931, or -465 to +465 when centered around 0)
// use y = mx + b
// m = (y2 - y1) / (x2 - x1)
// y2 = 465, y1 = -465, x2 = 2000, x1 = 1000
float mGyro = 0.93;
// b = y1 - m * x1
float bGyro = -1395;

// Scale transmitter commands (1000 to 2000) to motor commands (2000 to 4700)
// use y = mx + b
float mMotor = 2.7;
float bMotor = -700;

// Scale gyro rate (-465 to +465) to motor commands (2000 to 4700)
// use y = mx + b
float mMotorRate = 2.9;
float bMotorRate = 3348.5;

// Digital smoothing parameters
// Taken from http://www.arduino.cc/playground/Main/DigitalSmooth
#define FILTERSAMPLES 23               // filterSamples should be an odd number, no smaller than 3
int rollSmoothArray [FILTERSAMPLES];   // array for holding raw sensor values for roll 
int rollData, rollSmoothData;          // variables for roll data
int pitchSmoothArray [FILTERSAMPLES];  // array for holding raw sensor values for pitch 
int pitchData, pitchSmoothData;        // variables for pitch data
int yawSmoothArray [FILTERSAMPLES];    // array for holding raw sensor values for yaw 
int yawData, yawSmoothData;            // variables for yaw data

// PID Values
#define WINDUP_GUARD_GAIN 100.0
float pTerm, iTerm, dTerm; 
float pgain = 500;
float igain = 1;
float dgain = -450;
float iRollState = 0;
float lastRollPosition = 0;
float iPitchState = 0;
float lastPitchPosition = 0;
float iYawState = 0;
float lastYawPosition = 0;

// Communication
volatile char queryType = 'G';

// Interrupt Handler
volatile byte timeSlot;

// Timing
long previousTime = 0;
long currentTime = 0;
long deltaTime = 0;

// ******************** Setup MikroQuadAero ********************
void setup() {
  Serial.begin(BAUD);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  pinMode(LEDPIN, OUTPUT);
  pinMode(RESETPIN, OUTPUT);  
  pinMode(THROTTLEPIN, INPUT);
  pinMode(ROLLPIN, INPUT);
  pinMode(PITCHPIN, INPUT);
  pinMode(YAWPIN, INPUT);
  digitalWrite(LEDPIN, LOW);
  
  // Calibrate gyros
  rollGyroZero = gyroMid - findZero(ROLLRATEPIN);
  pitchGyroZero = gyroMid - findZero(PITCHRATEPIN);
  yawGyroZero = gyroMid - findZero(YAWRATEPIN);
  
  // Reset Pololu servo controller
  digitalWrite(RESETPIN, LOW);
  delay(1000);
  digitalWrite(RESETPIN, HIGH);

  // Configure motors
  for (byte channel = 0; channel < 4; channel++)
    motorConfigure(channel);

  // Wait for ESC's to initialize
  delay(4000);

  // Setup interrupt to trigger on pin D3 (roll receiver pin)
  attachInterrupt(1, interruptHandler, RISING);

  // Enable MikroQuadAero ready LED
  digitalWrite(LEDPIN, HIGH);
  previousTime = millis();
}

// ******************** Main MikroQuadAero Loop ********************
void loop () {
  // Measure Timing
  currentTime = millis();
  deltaTime = currentTime - previousTime;
  previousTime = currentTime;

  // Read Sensors
  // Force values to be centered at 0
  rollRate = digitalSmooth(analogRead(ROLLRATEPIN), rollSmoothArray) + rollGyroZero - gyroMid;
  pitchRate = digitalSmooth(analogRead(PITCHRATEPIN), pitchSmoothArray) + pitchGyroZero - gyroMid;
  yawRate = digitalSmooth(analogRead(YAWRATEPIN), yawSmoothArray) + yawGyroZero - gyroMid;
  
  // Calculate Commanded Angles
  // Convert from PWM pulse length to gyro A/D value
  rollCommand = (mGyro * roll) + bGyro;
  pitchCommand = (mGyro * pitch) + bGyro;
  yawCommand = (mGyro * yaw) + bGyro;
  
  // Update PID
  throttleMotorCommand = (mMotor * throttle) + bMotor;
  rollMotorCommand = (mMotorRate * updatePID(rollRate, rollCommand, &lastRollPosition, &iRollState)) + bMotorRate;
  pitchMotorCommand = (mMotorRate * updatePID(pitchRate, pitchCommand, &lastPitchPosition, &iPitchState)) + bMotorRate;
  yawMotorCommand = (mMotorRate * updatePID(yawRate, yawCommand, &lastYawPosition, &iYawState)) + bMotorRate;   

  // Command Motors
  frontCommand = constrain((unsigned int)(throttleMotorCommand + pitchMotorCommand + yawMotorCommand), 2000, 4700);
  rearCommand = constrain((unsigned int)(throttleMotorCommand - pitchMotorCommand + yawMotorCommand), 2000, 4700);
  rightCommand = constrain((unsigned int)throttleMotorCommand + rollMotorCommand - yawMotorCommand, 2000, 4700);
  leftCommand = constrain((unsigned int)(throttleMotorCommand - rollMotorCommand - yawMotorCommand), 2000, 4700);
}

int findZero(int channel) {
  int zero = 0;
  for (int i=0; i< 64; i++) zero += analogRead(channel);
  return zero >> 6;
}  



// Interrupts are generated every 20ms
// Modulo division by 5 causes each time slot to happen every 100ms

void interruptHandler() {
  switch ((timeSlot += 1) % 5) {
  case 0:
    // Read Receiver
    roll = pulseIn(ROLLPIN, HIGH, TIMEOUT);
    aux = pulseIn(AUXPIN, HIGH, TIMEOUT);
    pitch = pulseIn(PITCHPIN, HIGH, TIMEOUT);
    yaw = pulseIn(YAWPIN, HIGH, TIMEOUT);
    throttle = pulseIn(THROTTLEPIN, HIGH, TIMEOUT);
    gear = pulseIn(GEARPIN, HIGH, TIMEOUT);
    break;
  case 1:
    // Check Query
    if (Serial.available())
      queryType = Serial.read();
    break;
  case 2:
    // Send Data
    switch (queryType) {
    case'G':
      Serial.print(deltaTime);
      Serial.print(',');
      Serial.print(rollRate);
      Serial.print(',');
      Serial.print(pitchRate);
      Serial.print(',');
      Serial.println(yawRate);
      break;
    case 'P':
      Serial.print(deltaTime);
      Serial.print(',');
      Serial.print(int(pgain*1000));
      Serial.print(',');
      Serial.print(int(igain*1000));
      Serial.print(',');
      Serial.println(int(dgain*1000));
      break;
    }
  }
}

// This sketch interfaces with the Pololu servo controller at:
// http://www.pololu.com/catalog/product/207
// This code is written by Adam (nexisnet) at:
// http://forum.pololu.com/viewtopic.php?f=16&t=745&st=0&sk=t&sd=a&start=15

void motorCommand(byte servo, unsigned int angle){
   //servo is the servo number (typically 0-7)
   //angle is the absolute position from 2000 to 4695?

   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x04,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   //Convert the angle data into two 7-bit bytes
   Serial.print(((angle>>7)&0x3f),BYTE); //data1
   Serial.print((angle&0x7f),BYTE); //data2
}

void motorConfigure(byte servo){
   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x01,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   Serial.print(0x0,BYTE); //instant change
   
   motorCommand(servo, 2000);
}

// Taken from http://www.arduino.cc/playground/Main/BarebonesPIDForEspresso

float updatePID(float targetPosition, float currentPosition, float *lastPosition, float *iState)
{
  // these local variables can be factored out if memory is an issue, 
  // but they make it more readable
  float error;
  float windupGaurd;

  // determine how badly we are doing
  error = targetPosition - currentPosition;

  // the pTerm is the view from now, the pgain judges 
  // how much we care about error we are this instant.
  pTerm = pgain * error;

  // iState keeps changing over time; it's 
  // overall "performance" over time, or accumulated error
  *iState += error;

  // to prevent the iTerm getting huge despite lots of 
  //  error, we use a "windup guard" 

  // not necessary, but this makes windup guard values 
  // relative to the current iGain
  windupGaurd = WINDUP_GUARD_GAIN / igain;  

  if (*iState > windupGaurd) 
    *iState = windupGaurd;
  else if (*iState < -windupGaurd) 
    *iState = -windupGaurd;
  iTerm = igain * (*iState);

  // the dTerm, the difference between the current
  //  and last reading, indicated the "speed," 
  // how quickly the reading is changing. (aka. Differential)
  dTerm = (dgain * (currentPosition - *lastPosition));
  *lastPosition = currentPosition;
  return  pTerm + iTerm - dTerm;
}

// Taken from http://www.arduino.cc/playground/Main/DigitalSmooth

int digitalSmooth(int rawIn, int *sensSmoothArray) {
  // "int *sensSmoothArray" passes an array to the function - the asterisk indicates the array name is a pointer
  int j, k, temp, top, bottom;
  long total;
  static int i;
  static int sorted[FILTERSAMPLES];
  boolean done;

  i = (i + 1) % FILTERSAMPLES; // increment counter and roll over if necc. -  % (modulo operator) rolls over variable
  sensSmoothArray[i] = rawIn; // input new data into the oldest slot

  for (j=0; j<FILTERSAMPLES; j++){ // transfer data array into anther array for sorting and averaging
    sorted[j] = sensSmoothArray[j];
  }

  done = 0;                // flag to know when we're done sorting              
  while(done != 1){        // simple swap sort, sorts numbers from lowest to highest
    done = 1;
    for (j = 0; j < (FILTERSAMPLES - 1); j++){
      if (sorted[j] > sorted[j + 1]){     // numbers are out of order - swap
        temp = sorted[j + 1];
        sorted [j+1] =  sorted[j] ;
        sorted [j] = temp;
        done = 0;
      }
    }
  }
  // throw out top and bottom 15% of samples - limit to throw out at least one from top and bottom
  bottom = max(((FILTERSAMPLES * 15) / 100), 1); 
  top = min((((FILTERSAMPLES * 85) / 100) + 1), (FILTERSAMPLES - 1));   // the + 1 is to make up for asymmetry caused by integer rounding
  k = 0;
  total = 0;
  for ( j = bottom; j< top; j++){
    total += sorted[j];  // total remaining indices
    k++; 
  }
  return total / k;    // divide by number of samples
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

